/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.ortn.wildbots.subsystems;

import edu.ortn.wildbots.RobotMap;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
 *
 * @author Stephen
 */
public class Elevator extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    
    public static final int NUM_CHAMBERS = 3;
    public static final double SPEED = 1;
    
    private Jaguar motor1;
    private Jaguar motor2;
    private boolean isMotor1Started;
    private boolean isMotor2Started;
    private DigitalInput sensorTop, sensorMiddle, sensorBottom;
    
    public Elevator() {
        sensorBottom = new DigitalInput(RobotMap.CHAMBER_SENSOR_START);
        sensorMiddle = new DigitalInput(RobotMap.CHAMBER_SENSOR_START+1);
        sensorTop = new DigitalInput(RobotMap.CHAMBER_SENSOR_START+2);
        
        motor1 = new Jaguar(RobotMap.CHAMBER_MOTOR_START);
        motor2 = new Jaguar(RobotMap.CHAMBER_MOTOR_START+1);
    }
    
    public void initDefaultCommand() {}
    
    public void startMotor(int num) {
       if (num == 1) {
           if (!isMotor1Started) { 
            isMotor1Started = true;
            motor1.set(SPEED);
           }
       }
       
       else if (num == 2){
           if (!isMotor2Started) {
                isMotor2Started = true;
                motor2.set(SPEED);
           }
       }
    }
    
    public void stopMotor(int num) {
        if (num == 1) {
            if (isMotor1Started) {
                motor1.set(0.0);
                isMotor1Started = false;
            }
        }
        else if (num == 2){
            if (isMotor2Started) {
                motor2.set(0.0);
                isMotor2Started = false;
            }  
        }      
    }
    
    public void update(){
        if (!sensorTop.get()) { 
                startMotor(1);
                startMotor(2);
        } 
        else if (!sensorMiddle.get()) {
                startMotor(1);
                startMotor(2);
        }
        else if (!sensorBottom.get()) {
                startMotor(1);
                stopMotor(2);
        }
        else if (sensorBottom.get() && sensorMiddle.get() && sensorTop.get()) {
                stopMotor(1);
                stopMotor(2);
        }
        else {
            startMotor(1);
            startMotor(2);
        }
        
    }
    
    public void reset() {
        stopMotor(1);
        stopMotor(2);
    }
    
    public String[] getData() {
        String[] data = new String[5];
        data[0] = "Bottom Chamber: " + ((sensorBottom.get())? "pressed": "unpressed");
        data[1] = "Middle Chamber: " + ((sensorMiddle.get())? "pressed": "unpressed");
        data[2] = "Top Chamber: " + ((sensorTop.get())? "pressed": "unpressed");
        data[3] = "Motor 1: " + motor1.getSpeed();
        data[4] = "Motor 2: " + motor2.getSpeed();
        return data;
    }
}
